#include "discharge_soc_calibration.h"

#include "../windows_service/board/port.h"
#include "../windows_service/tlog/tlog.h"
#include "stdlib.h"

/***************************************************************************************************
 * ****************************************测试使用的参数***************************************** *
 ***************************************************************************************************/
#define TAG                                      "discharge_soc_calibration"
#define cell_voltage_protection                  2500  // 单体保护2.5V
#define cell_voltage_protection_temp_less_than_0 2000  // 单体保护2V

typedef enum {
  interval_nonexistent = -1,  // 不存在区间(该模块中表示比数据表格中的最小值还要小)
  interval_data_same   = 0,   // 区间数据相同
} interval_t;

static discharge_soc_calibration_t* table_s                   = NULL;
static uint8                        table_size_s              = 0;
static float*                       now_temp_cell_min_voltage = NULL; /* 当前温度单体最低电压 */

static float get_cell_voltage_protection(int8 temp, uint16 cell_voltage_mv) {
  if (temp > 0) return (float)cell_voltage_mv / 1000;
  return (float)cell_voltage_protection_temp_less_than_0 / 1000;
}

// static float get_cell_voltage_protection(void) { return (float)cell_voltage_protection_temp_less_than_0 / 1000; }

static int8 get_table_size(int8 temp) {
  /* 遍历不同温度下电池容量表，并根据温度返回区间高数组下标 */
  for (uint8 index = 0; index < table_size_s; index++) {
    if (temp <= table_s[index].temp) {
      return index;
    }
  }
  return table_size_s;  // 表格的最大值
}

static float calculate_slope(float x1, float y1, float x2, float y2) {
  if (x2 - x1 != 0) {  // 避免除以零的情况
    float slope = (y2 - y1) / (x2 - x1);
    return slope;
  } else {
    return 0;  // 当x2 = x1时，斜率不存在，默认返回0
  }
}

static float calc_cell_voltage(int8 temp_now, uint8 table_interval_max_index, uint8 table_interval_min_index, uint8 soc_cell_table_size) {
  /* 计算随着温度变化的单体电压值 */
  float slope_cal_r;
  float cell_voltage = 0;
  float temp         = 0; /* 临时变量 */

  if (table_interval_max_index == table_size_s) {
    cell_voltage = table_s[table_size_s - 1].soc_cell_table[soc_cell_table_size].cell_voltage;
  } else {
    if (table_interval_max_index > 0) {
      slope_cal_r = calculate_slope(table_s[table_interval_max_index].temp,                                              // x1
                                    table_s[table_interval_max_index].soc_cell_table[soc_cell_table_size].cell_voltage,  // y1
                                    table_s[table_interval_min_index].temp,                                              // x2
                                    table_s[table_interval_min_index].soc_cell_table[soc_cell_table_size].cell_voltage);
    } else {
      slope_cal_r = interval_nonexistent;
    }

    if (slope_cal_r > 0) {
      temp = MATH_ABS_VAL(table_s[table_interval_min_index].temp - temp_now) * slope_cal_r;  // 计算出对应温度需要增加的容量(从区间最小值加)
      cell_voltage = table_s[table_interval_min_index].soc_cell_table[soc_cell_table_size].cell_voltage + temp;
    } else if (slope_cal_r == interval_nonexistent) {
      cell_voltage = table_s[0].soc_cell_table[soc_cell_table_size].cell_voltage;
    } else if (slope_cal_r == interval_data_same) {
      cell_voltage = table_s[table_interval_max_index].soc_cell_table[soc_cell_table_size].cell_voltage;
    }
  }

  return cell_voltage;
}

static BOOLEAN table_init_malloc(void) {
  if (table_s->soc_cell_table_size > 0) {
    now_temp_cell_min_voltage = (float*)malloc(table_s->soc_cell_table_size * sizeof(float));
  }
}

static int8 get_soc_interval_index(float cell_min_voltage) {
  /* 获取soc区间数组下标 */
  for (uint8 index = 0; index < (table_s->soc_cell_table_size - 1); index++) {  // table_s->soc_cell_table_size-1 是因为获取范围，3个数据只有2个范围
    // TLOGI(TAG, "cell_min_voltage:%f,up:%f,down:%f\n", cell_min_voltage, now_temp_cell_min_voltage[index], now_temp_cell_min_voltage[index + 1]);
    if (cell_min_voltage <= now_temp_cell_min_voltage[index] &&  //
        cell_min_voltage >= now_temp_cell_min_voltage[index + 1]) {
      return index + 1; /* 返回区间下标大的那一位 */
    }
  }
  return calibration_err;
}

static float soc_and_cell_voltage_slope_cal(int8 table_max_index, int8 interval_max_index) {
  /* 斜率计算,K = (y1 - y2) / (x1 - x2) */
  /* y = kx */
  float slope_r = 0.0;

  if (interval_max_index > 0) {
    // TLOGI(TAG, "x1:%d,y1:%f,x2:%d,y2:%f\n",                                         //
    //       table_s[table_max_index].soc_cell_table[interval_max_index].soc,          // x1
    //       now_temp_cell_min_voltage[interval_max_index],                            // y1
    //       table_s[table_max_index - 1].soc_cell_table[interval_max_index - 1].soc,  // x2
    //       now_temp_cell_min_voltage[interval_max_index - 1]                         // y2
    // );

    slope_r = calculate_slope(now_temp_cell_min_voltage[interval_max_index],                           // x1
                              table_s[table_max_index].soc_cell_table[interval_max_index].soc,         // y1
                              now_temp_cell_min_voltage[interval_max_index - 1],                       // x2
                              table_s[table_max_index - 1].soc_cell_table[interval_max_index - 1].soc  // y2
    );
    // TLOGI(TAG, "slope_r:%f\n", slope_r);
  } else if (interval_max_index == 0) {
    // 表格边界处理
    slope_r = -1;  // 此时最低温度超过最小值，那么再低的温度，此时容量也是70%
  }
  return slope_r;
}

int16 soc_value_of_the_current_voltage_calc(int8 table_max_index, float cell_min_voltage) {
  /* 根据当前的单体电压计算SOC */
  float slope_cal_r;
  float temp = 0; /* 临时变量 */
  int16 soc  = 0;
  int8  soc_interval_max_index;

  if (cell_min_voltage >= now_temp_cell_min_voltage[0]) {
    /* 当前单体电压比SOC25的单体电压还要高，设置SOC为25 */
    soc = table_s->soc_cell_table[0].soc;
  } else {
    soc_interval_max_index = get_soc_interval_index(cell_min_voltage);
    // TLOGI(TAG, "soc_interval_max_index:%d\n", soc_interval_max_index);
    if (soc_interval_max_index > 0) {
      slope_cal_r = calculate_slope(now_temp_cell_min_voltage[soc_interval_max_index],                           // x1
                                    table_s[table_max_index].soc_cell_table[soc_interval_max_index].soc,         // y1
                                    now_temp_cell_min_voltage[soc_interval_max_index - 1],                       // x2
                                    table_s[table_max_index - 1].soc_cell_table[soc_interval_max_index - 1].soc  // y2
      );
      // TLOGI(TAG, "slope_cal_r:%f\n", slope_cal_r);
      if (slope_cal_r > 0) {
        temp = MATH_ABS_VAL(now_temp_cell_min_voltage[soc_interval_max_index] - cell_min_voltage) * slope_cal_r;
        // TLOGI(TAG, "temp:%f,now_temp_min_vol:%f,cell_min_voltage:%f\n", temp, now_temp_cell_min_voltage[soc_interval_max_index], cell_min_voltage);
        soc = table_s[table_max_index].soc_cell_table[soc_interval_max_index].soc + temp;
        // TLOGI(TAG, "soc:%d,tab_soc:%d\n", soc, table_s[table_max_index].soc_cell_table[soc_interval_max_index].soc);
      }
      /* 下面两个条件不会进去，暂时屏蔽 */
      //  else if (slope_cal_r == interval_nonexistent) {
      //   soc = table_s[table_max_index].soc_cell_table[soc_interval_max_index].soc;
      // } else if (slope_cal_r == interval_data_same) {
      //   soc = table_s[table_max_index].soc_cell_table[soc_interval_max_index - 1].soc;
      // }
    } else {
      /* 不在表格要求的SOC，不进行校准 */
      return calibration_err;
    }
  }

  return soc;
}

void discharge_soc_calibration_init(discharge_soc_calibration_t* table, uint8 table_size) {
  table_s      = table;
  table_size_s = table_size;
  table_init_malloc();
  return;
}

/* 返回calibration_err表示校准失败 */
int16 discharge_soc_calibration_calibrate(int8 temp_now, int16 soc_now, float cell_min_voltage) {
  /* 该校准只存在于放电时候才进行校准 */
  /* 注意：该校准不能和SOC下限校准冲突，否则无法做。 */
  int16 soc                      = 0;
  int8  table_interval_max_index = get_table_size(temp_now);
  int8  table_interval_min_index = table_interval_max_index - 1;
  int8  soc_interval_max_index;

  if (table_s == NULL ||                             // 没有init
      soc_now > table_s[0].soc_cell_table[0].soc) {  // SOC没到进入校准的阈值(请确保soc_cell_table数组的所有SOC数据一样)
    return calibration_err;                          // 不允许校准，返回ERROR
  }
  TLOGI(TAG," table_s[0].soc_cell_table[0].soc:%d\n", table_s[0].soc_cell_table[0].soc);
  // if (cell_min_voltage < get_cell_voltage_protection(temp_now, cell_voltage_protection)) return calibration_err;

  for (uint8 index = 0; index < table_s->soc_cell_table_size; index++) {
    /* 计算出当前温度下，表格中所有的SOC阶段最低单体电压 */
    now_temp_cell_min_voltage[index] = calc_cell_voltage(temp_now, table_interval_max_index, table_interval_min_index, index);
  }
  // TLOGI(TAG, "temp:%d,vol[0]:%f,vol[1]:%f,vol[2]:%f\n", temp_now, now_temp_cell_min_voltage[0], now_temp_cell_min_voltage[1], now_temp_cell_min_voltage[2]);

  soc = soc_value_of_the_current_voltage_calc(table_interval_max_index, cell_min_voltage);

  return soc;
}